﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Drawing;
using System.Drawing.Imaging;


/// <summary>
/// ObjectDetection命名空间实现导游机器人工作环境中各种目标的检测
/// @author Jie Deng
/// @version 1.0
/// </summary>
namespace ObjectDetection
{
    /// <summary>
    /// LineDetection是实现直线检测的类
    /// </summary>
    public class LineDetection
    {
        private Bitmap mybitmap;    //需要处理的图像


        /**
         * 无参私有构造函数
         */
        private LineDetection() { }


        /**
         * 构造函数
         * @param Bitmap
         */
        public LineDetection(Bitmap mybitmap)
        {
            this.mybitmap = mybitmap;
        }


        /**
         * 构造函数
         * @param string
         */
        public LineDetection(string bitmappath)
        {
            this.mybitmap = (Bitmap)Image.FromFile(bitmappath);
        }


        /// <summary>
        /// @return int类型，返回偏转角度-90-90
        /// </summary>
        public int GetAngle()
        {
            int angle = -1;
            Hough_Line1(ref angle);
            return angle;
        }


        /// <summary>
        /// Hough_Line方法检测直线
        /// @param int类型，为检测出的直线的角度,0-180,没有直线返回-1
        /// @return Bitmap类型，为检测出直线后重构出的图
        /// </summary>
        private Bitmap Hough_Line(ref int angle)
        {
            byte thres = 50;
            Bitmap curBitmap = BinaryImage(mybitmap, thres);

            int x = curBitmap.Width;
            int y = curBitmap.Height;
            //由原图数组坐标算出ρ最大值，并取整数部分加1
            int rho_max = (int)Math.Floor(Math.Sqrt(x * x + y * y)) + 1;

            /*此值作为ρ，θ坐标系ρ最大值
             *定义ρ，θ坐标系的数组，初值为0。θ的最大值，180度
             */
            int[,] accarray = new int[rho_max, 180];
            double[] Theta = new double[180];
            //定义θ数组，确定θ取值范围
            double i = 0;
            for (int index = 0; index < 180; index++)
            {
                Theta[index] = i;
                i += Math.PI / 180;
            }

            double rho;
            int rho_int;
            for (int n = 0; n < x; n++)
            {
                for (int m = 0; m < y; m++)
                {
                    Color pixel = curBitmap.GetPixel(n, m);
                    if ((pixel.R + pixel.B + pixel.G) / 3 < 5)
                    {
                        for (int k = 0; k < 180; k++)
                        {
                            //将θ值代入hough变换方程，求ρ值
                            rho = (m * Math.Cos(Theta[k])) + (n * Math.Sin(Theta[k]));
                            //将ρ值与ρ最大值的和的一半作为ρ的坐标值（数组坐标），这样做是为了防止ρ值出现负数
                            rho_int = (int)Math.Round(rho / 2 + rho_max / 2);
                            //在ρθ坐标（数组）中标识点，即计数累加
                            accarray[rho_int, k]++;
                        }
                    }
                }
            }

            //直线检测门限,hough变换后的曲线交点个数，取值越大，找出的直线越少
            int cross_num = 0;

            //寻找accarray中的最大值
            foreach (int t in accarray)
            {
                if (t > cross_num)
                {
                    cross_num = t;
                }
            }

            //=======利用hough变换提取直线======
            //寻找100个以内符合要求的点
            const int max_line = 100;
            int[] case_accarray_n = new int[max_line];
            int[] case_accarray_m = new int[max_line];
            int K = 0; //存储数组计数器
            for (int rho_n = 0; rho_n < rho_max; rho_n++) //在hough变换后的数组中搜索
            {
                for (int theta_m = 0; theta_m < 180; theta_m++)
                {
                    if (accarray[rho_n, theta_m] >= (cross_num / 2 + 5) && K < max_line) //设定直线的最小值
                    {
                        case_accarray_n[K] = rho_n; //存储搜索出的数组下标
                        case_accarray_m[K] = theta_m;
                        K++;
                    }
                }
            }

            //把这些点构成的直线提取出来,输出图像数组为I_out
            //I_out=ones(x,y).*255;
            Bitmap I_out = new Bitmap(x, y);

            for (int n = 0; n < x; n++)
            {
                for (int m = 0; m < y; m++)
                {
                    //首先设置为白色
                    I_out.SetPixel(n, m, Color.White);
                    Color pixel = curBitmap.GetPixel(n, m);
                    if ((pixel.R + pixel.B + pixel.G) / 3 < 5)
                    {
                        for (int k = 0; k < 180; k++)
                        {
                            rho = (m * Math.Cos(Theta[k])) + (n * Math.Sin(Theta[k]));
                            rho_int = (int)Math.Round(rho / 2 + rho_max / 2);
                            //取出最多100条直线
                            if (K==0)
                            {
                                angle = -1;
                            } 
                            else
                            {
                                for (int a = 0; a < K; a++)
                                {
                                    if (rho_int == case_accarray_n[a] && k == case_accarray_m[a])
                                    {
                                        I_out.SetPixel(n, m, Color.Black);
                                        angle = case_accarray_m[a];
                                    }
                                }
                            }
                            
                        }
                    }

                }
            }
            return I_out;
        }


        /// <summary>
        /// Hough_Line1方法检测直线,速度上优于Hough_Line
        /// @param int类型，为检测出的直线的角度,0-180,没有直线返回-1
        /// @return Bitmap类型，为检测出直线后重构出的图
        /// </summary>
        private Bitmap Hough_Line1(ref int angle)
        {
            byte thres = 50;
            Bitmap curBitmap = BinaryImage(mybitmap,thres);

            int x = curBitmap.Width;
            int y = curBitmap.Height;
            //由原图数组坐标算出ρ最大值，并取整数部分加1
            int rho_max = (int)Math.Floor(Math.Sqrt(x * x + y * y)) + 1;

            /*此值作为ρ，θ坐标系ρ最大值
             *定义ρ，θ坐标系的数组，初值为0。θ的最大值，180度
             */
            int[,] accarray = new int[rho_max, 180];
            double[] Theta = new double[180];
            //定义θ数组，确定θ取值范围
            double i = 0;
            for (int index = 0; index < 180; index++)
            {
                Theta[index] = i;
                i += Math.PI / 180;
            }


            Rectangle rect = new Rectangle(0, 0, curBitmap.Width, curBitmap.Height);
            //以可读写的方式锁定全部位图像素
            BitmapData bmpData = curBitmap.LockBits(rect, ImageLockMode.ReadOnly, curBitmap.PixelFormat);
            //得到首地址
            IntPtr ptr = bmpData.Scan0;

            //定义被锁定的数字大小，由位图数据与未用空间组成
            int bytes = bmpData.Stride * bmpData.Height;
            //txtTest.Text = bytes.ToString();
            byte[] rgbValues = new byte[bytes];
            //复制被锁定的位图像素值到该数组内
            System.Runtime.InteropServices.Marshal.Copy(ptr, rgbValues, 0, bytes);


            double rho;
            int rho_int;

            double colorTemp = 0;

            for (int n = 0; n < bmpData.Width * 3; n += 3)
            {
                //只处理每行中是图像像素的数据，舍弃未用空间
                for (int m = 0; m < bmpData.Height; m++)
                {
                    colorTemp = (rgbValues[m * bmpData.Stride + n + 2] + rgbValues[m * bmpData.Stride + n + 1] +
                        rgbValues[m * bmpData.Stride + n]) / 3;
                    if (colorTemp < 5)
                    {
                        for (int k = 0; k < 180; k++)
                        {
                            //将θ值代入hough变换方程，求ρ值
                            rho = (m * Math.Cos(Theta[k])) + (n / 3 * Math.Sin(Theta[k]));
                            //将ρ值与ρ最大值的和的一半作为ρ的坐标值（数组坐标），这样做是为了防止ρ值出现负数
                            rho_int = (int)Math.Round(rho / 2 + rho_max / 2);
                            //在ρθ坐标（数组）中标识点，即计数累加
                            accarray[rho_int, k]++;
                        }
                    }
                }
            }


            //直线检测门限,hough变换后的曲线交点个数，取值越大，找出的直线越少
            int cross_num = 0;

            //寻找accarray中的最大值
            foreach (int t in accarray)
            {
                if (t > cross_num)
                {
                    cross_num = t;
                }
            }


            //=======利用hough变换提取直线======
            // 寻找100个以内符合要求的点
            const int max_line = 100;
            int[] case_accarray_n = new int[max_line];
            int[] case_accarray_m = new int[max_line];
            int K = 0; //存储数组计数器
            for (int rho_n = 0; rho_n < rho_max; rho_n++) //在hough变换后的数组中搜索
            {
                for (int theta_m = 0; theta_m < 180; theta_m++)
                {
                    if (accarray[rho_n, theta_m] >= cross_num / 2 + 5 && K < max_line) //设定直线的最小值
                    {
                        case_accarray_n[K] = rho_n; //存储搜索出的数组下标
                        case_accarray_m[K] = theta_m;
                        K++;
                    }
                }
            }

            //把这些点构成的直线提取出来,输出图像数组为I_out
            //I_out=ones(x,y).*255;
            Bitmap I_out = new Bitmap(x, y);


            Rectangle rect1 = new Rectangle(0, 0, I_out.Width, I_out.Height);
            //以可读写的方式锁定全部位图像素
            BitmapData bmpData1 = I_out.LockBits(rect1, ImageLockMode.ReadWrite, I_out.PixelFormat);
            //得到首地址
            IntPtr ptr1 = bmpData1.Scan0;

            //定义被锁定的数字大小，由位图数据与未用空间组成
            int bytes1 = bmpData1.Stride * bmpData1.Height;
            //txtTest.Text = bytes.ToString();
            byte[] rgbValues1 = new byte[bytes1];
            //复制被锁定的位图像素值到该数组内
            System.Runtime.InteropServices.Marshal.Copy(ptr1, rgbValues1, 0, bytes1);


            colorTemp = 0;
            for (int n = 0; n < bmpData1.Width * 3; n += 3)
            {
                //只处理每行中是图像像素的数据，舍弃未用空间
                for (int m = 0; m < bmpData1.Height; m++)
                {
                    rgbValues1[m * bmpData1.Stride + n] = rgbValues1[m * bmpData1.Stride + n + 1] =
                            rgbValues1[m * bmpData1.Stride + n + 2] = (byte)255;

                    colorTemp = (rgbValues[m * bmpData.Stride + n + 2] + rgbValues[m * bmpData.Stride + n + 1] +
                        rgbValues[m * bmpData.Stride + n]) / 3;

                    if (colorTemp < 5)
                    {
                        for (int k = 0; k < 180; k++)
                        {
                            rho = (m * Math.Cos(Theta[k])) + (n / 3 * Math.Sin(Theta[k]));
                            rho_int = (int)Math.Round(rho / 2 + rho_max / 2);
                            //如果正在计算的点属于100像素以上点，则把它提取出来
                            if (K==0)
                            {
                                angle = -1;
                            }
                            else
                            {
                                for (int a = 0; a < K; a++)
                                {
                                    if (rho_int == case_accarray_n[a] && k == case_accarray_m[a])
                                    {
                                        rgbValues1[m * bmpData1.Stride + n] = rgbValues1[m * bmpData1.Stride + n + 1] =
                                            rgbValues1[m * bmpData1.Stride + n + 2] = (byte)0;
                                        angle = case_accarray_m[a];
                                    }
                                }
                            }
                        }
                    }
                }
            }

            //把数组复制回位图
            System.Runtime.InteropServices.Marshal.Copy(rgbValues1, 0, ptr1, bytes1);

            //解锁位图像素
            curBitmap.UnlockBits(bmpData);
            I_out.UnlockBits(bmpData1);

            return I_out;
        }


        /*
        /// <summary>
        /// BinaryImage方法对图像二值化处理
        /// @param double类型，为二数化的阈值
        /// @return Bitmap类型,二值化后的图像
        /// </summary>
        /// <param name="thres">二值化阈值</param>
        private Bitmap BinaryImage(double thres)
        {
            Bitmap curBitmap = new Bitmap(mybitmap);
            Rectangle rect = new Rectangle(0, 0, curBitmap.Width, curBitmap.Height);
            //以可读写的方式锁定全部位图像素
            BitmapData bmpData = curBitmap.LockBits(rect, ImageLockMode.ReadWrite, curBitmap.PixelFormat);
            //得到首地址
            IntPtr ptr = bmpData.Scan0;

            //定义被锁定的数字大小，由位图数据与未用空间组成
            int bytes = bmpData.Stride * bmpData.Height;
            //txtTest.Text = bytes.ToString();
            byte[] rgbValues = new byte[bytes];
            //复制被锁定的位图像素值到该数组内
            System.Runtime.InteropServices.Marshal.Copy(ptr, rgbValues, 0, bytes);

            double colorTemp = 0;
            for (int i = 0; i < bmpData.Height; i++)
            {
                //只处理每行中是图像像素的数据，舍弃未用空间
                for (int j = 0; j < bmpData.Width * 3; j += 3)
                {
                    colorTemp = (rgbValues[i * bmpData.Stride + j + 2] + rgbValues[i * bmpData.Stride + j + 1] +
                        rgbValues[i * bmpData.Stride + j]) / 3;
                    if (colorTemp<thres)
                    {
                        rgbValues[i * bmpData.Stride + j] = rgbValues[i * bmpData.Stride + j + 1] =
                            rgbValues[i * bmpData.Stride + j + 2] = (byte)0;
                    }
                    else
                    {
                        rgbValues[i * bmpData.Stride + j] = rgbValues[i * bmpData.Stride + j + 1] =
                            rgbValues[i * bmpData.Stride + j + 2] = (byte)255;
                    }
                    
                }
            }

            //把数组复制回位图
            System.Runtime.InteropServices.Marshal.Copy(rgbValues, 0, ptr, bytes);

            //解锁位图像素
            curBitmap.UnlockBits(bmpData);

            //返回处理后的图像
            return curBitmap;
        }
        */


        /// <summary>
        /// 给定阈值法二值化模块
        /// </summary>
        /// <param name="b"></param>
        /// <param name="threshold"></param>
        /// <returns></returns>
        private Bitmap BinaryImage(Bitmap b, byte threshold)
        {
            int width = b.Width;
            int height = b.Height;
            BitmapData data = b.LockBits(new Rectangle(0, 0, width, height), ImageLockMode.ReadWrite, PixelFormat.Format32bppArgb);
            unsafe
            {
                byte* p = (byte*)data.Scan0;
                int offset = data.Stride - width * 4;
                byte R, G, B, gray;
                for (int y = 0; y < height; y++)
                {
                    for (int x = 0; x < width; x++)
                    {
                        R = p[2];
                        G = p[1];
                        B = p[0];
                        gray = (byte)((R * 19595 + G * 38469 + B * 7472) >> 16);
                        if (gray >= threshold)
                        {
                            p[0] = p[1] = p[2] = 255;
                        }
                        else
                        {
                            p[0] = p[1] = p[2] = 0;
                        }
                        p += 4;
                    }
                    p += offset;
                }
                b.UnlockBits(data);
                return b;
            }
        }


        /// <summary>
        /// Otsu阈值法二值化模块 
        /// </summary>
        /// <param name="b"></param>
        /// <returns></returns>
        private Bitmap BinaryImage(Bitmap b)
        {
            // 图像灰度化   
            // b = Gray(b);   
            int width = b.Width;
            int height = b.Height;
            byte threshold = 0;
            int[] hist = new int[256];
            int AllPixelNumber = 0, PixelNumberSmall = 0, PixelNumberBig = 0;
            double MaxValue, AllSum = 0, SumSmall = 0, SumBig, ProbabilitySmall, ProbabilityBig, Probability;
            BitmapData data = b.LockBits(new Rectangle(0, 0, width, height), ImageLockMode.ReadWrite, PixelFormat.Format32bppArgb);
            unsafe
            {
                byte* p = (byte*)data.Scan0;
                int offset = data.Stride - width * 4;
                for (int j = 0; j < height; j++)
                {
                    for (int i = 0; i < width; i++)
                    {
                        hist[p[0]]++;
                        p += 4;
                    }
                    p += offset;
                }
                b.UnlockBits(data);
            }
            //计算灰度为I的像素出现的概率   
            for (int i = 0; i < 256; i++)
            {
                AllSum += i * hist[i];     //   质量矩   
                AllPixelNumber += hist[i];  //  质量       
            }
            MaxValue = -1.0;
            for (int i = 0; i < 256; i++)
            {
                PixelNumberSmall += hist[i];
                PixelNumberBig = AllPixelNumber - PixelNumberSmall;
                if (PixelNumberBig == 0)
                {
                    break;
                }
                SumSmall += i * hist[i];
                SumBig = AllSum - SumSmall;

                ProbabilitySmall = SumSmall / PixelNumberSmall;

                ProbabilityBig = SumBig / PixelNumberBig;

                Probability = PixelNumberSmall * ProbabilitySmall * ProbabilitySmall + PixelNumberBig * ProbabilityBig * ProbabilityBig;
                if (Probability > MaxValue)
                {
                    MaxValue = Probability;
                    threshold = (byte)i;
                }
            }
            return this.BinaryImage(b, threshold);
        } // end of OtsuThreshold 2  


        public sbyte GetScale()
        {
            byte thres = 50;
            Bitmap curBitmap = BinaryImage(mybitmap, thres);

            Rectangle rect = new Rectangle(0, 0, curBitmap.Width, curBitmap.Height);
            //以可读写的方式锁定全部位图像素
            BitmapData bmpData = curBitmap.LockBits(rect, ImageLockMode.ReadOnly, curBitmap.PixelFormat);
            //得到首地址
            IntPtr ptr = bmpData.Scan0;
            //定义被锁定的数字大小，由位图数据与未用空间组成
            int bytes = bmpData.Stride * bmpData.Height;
            //txtTest.Text = bytes.ToString();
            byte[] rgbValues = new byte[bytes];
            //复制被锁定的位图像素值到该数组内
            System.Runtime.InteropServices.Marshal.Copy(ptr, rgbValues, 0, bytes);


            int x = bmpData.Width * 3;
            int y = bmpData.Height;

            int[] temp = { x, x };
            int low = y / 4;
            int high = y / 4 * 3;
            int[] J = { low, high };

            double colorTemp = 0;
            int t = 0;
            foreach (int j in J)
            {
                for (int i = 0; i < x; i += 3)
                {
                    colorTemp = (rgbValues[j * bmpData.Stride + i + 2] + rgbValues[j * bmpData.Stride + i + 1] +
                            rgbValues[j * bmpData.Stride + i]) / 3;

                    if (colorTemp < 10)//为黑色
                    {
                        temp[t++] = i;
                        break;
                    }
                }
            }

            double a = temp[1] - temp[0];
            a = a  * 5 * 127 / x;
            
            if(a>127)
            {
                a = 127; 
            }
            else if(a<-127)
            {
                a = -127;
            }
            else
            {
                a = a + 0;
            }

            curBitmap.UnlockBits(bmpData);

            return Convert.ToSByte(a);

        }

    }
}
